
#pragma once

#include <algorithm>
#include <array>
#include <memory>
#include <mutex>
#include <thread>
#include <unordered_map>
#include <vector>
#include "can_client.h"

namespace canbus
{

  template <typename SensorType>
  class CanSender
  {
  public:
    CanSender() = default;

    virtual ~CanSender() = default;

    ErrorCode Init(CanClient *can_client, bool enable_log);

    ErrorCode Start();

    void Update();
    void Stop();
    bool IsRunning() const;
    bool enable_log() const;

  private:
    void PowerSendThreadFunc();
    bool is_init_ = false;
    bool is_running_ = false;

    CanClient *can_client_ = nullptr; // Owned by global canbus.cc
    // std::vector<SenderMessage<SensorType>> send_messages_;
    std::unique_ptr<std::thread> thread_;
    bool enable_log_ = false;

  };

  const uint32_t kSenderInterval = 6000;

  template <typename SensorType>
  void CanSender<SensorType>::PowerSendThreadFunc()
  {
    sched_param sch;
    sch.sched_priority = 99;
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch);

    const int32_t INIT_PERIOD = 5000; // 5ms
    int32_t delta_period = INIT_PERIOD;
    int32_t new_delta_period = INIT_PERIOD;

    int64_t tm_start = 0;
    int64_t tm_end = 0;
    int64_t sleep_interval = 0;

    std::cout << "Can client sender thread starts.";

    while (is_running_)
    {
      // tm_start = cyber::Time::Now().ToNanosecond() / 1e3;
      // new_delta_period = INIT_PERIOD;

        // std::vector<CanFrame> frames // 取出数据发送
        std::vector<CanFrame> can_frames;
        if (can_client_->SendSingleFrame(can_frames) != ErrorCode::OK)
        {
          std::cout << "Send msg failed:" ;//<< can_frames.data;
        }
        if (enable_log())
        {
          std::cout << "send_can_frame#";// << can_frames.data;
        }
      // delta_period = new_delta_period;
      // tm_end = cyber::Time::Now().ToNanosecond() / 1e3;
      // sleep_interval = delta_period - (tm_end - tm_start);


      // if (sleep_interval > 0)
      // {
        std::this_thread::sleep_for(std::chrono::microseconds(sleep_interval));
      // }
      // else
      // {
      //   // do not sleep
      //   std::cout<< "Too much time for calculation: " << tm_end - tm_start
      //         << "us is more than minimum period: " << delta_period << "us";
      // }
    }
    std::cout << "Can client sender thread stopped!";
  }

  template <typename SensorType>
  ErrorCode CanSender<SensorType>::Init(CanClient *can_client,
                                        bool enable_log)
  {
    if (is_init_)
    {
      std::cout << "Duplicated Init request.";
      return ErrorCode::CANBUS_ERROR;
    }
    if (can_client == nullptr)
    {
      std::cout << "Invalid can client.";
      return ErrorCode::CANBUS_ERROR;
    }
    is_init_ = true;
    can_client_ = can_client;
    enable_log_ = enable_log;
    return ErrorCode::OK;
  }


  template <typename SensorType>
  ErrorCode CanSender<SensorType>::Start()
  {
    if (is_running_)
    {
      std::cout << "Cansender has already started.";
      return ErrorCode::CANBUS_ERROR;
    }
    is_running_ = true;
    thread_.reset(new std::thread([this]
                                  { PowerSendThreadFunc(); }));

    return ErrorCode::OK;
  }

  template <typename SensorType>
  void CanSender<SensorType>::Stop()
  {
    if (is_running_)
    {
      std::cout << "Stopping can sender ...";
      is_running_ = false;
      if (thread_ != nullptr && thread_->joinable())
      {
        thread_->join();
      }
      thread_.reset();
    }
    else
    {
      std::cout << "CanSender is not running.";
    }

    std::cout << "Can client sender stopped [ok].";
  }

  template <typename SensorType>
  bool CanSender<SensorType>::IsRunning() const
  {
    return is_running_;
  }

  template <typename SensorType>
  bool CanSender<SensorType>::enable_log() const
  {
    return enable_log_;
  }

} // namespace canbus
